#!/usr/bin/env python3


# read GY-86 10DOF board via i2c, mirror values to HAL pins
# 3 axis gyroscope + 3 accelerometer + 3 axis magnetic field+air pressure+temperature
# eg http://smart-prototyping.com/GY-86-10DOF-MS5611-HMC5883L-MPU6050-module-MWC-flight-control-sensor-module.html

# from: https://www.snip2code.com/Snippet/85741/Beaglebone-GY-86-driver

# git clone https://github.com/adafruit/adafruit-beaglebone-io-python.git
# cd adafruit-beaglebone-io-python/
# sudo make install
# sudo apt install python-smbus

from Adafruit_I2C import Adafruit_I2C
import math
import time
import argparse
from machinekit.hal import pyhal as hal


def main():
    parser = argparse.ArgumentParser(
        description='HAL component to read LSM303 Accelerometer values')
    parser.add_argument(
        '-n', '--name', help='HAL component name', default="gy86")
    parser.add_argument('-b', '--bus_id', help='I2C bus id', default=2)
    parser.add_argument('-i', '--interval',
                        help='I2C update interval', default=0.05)
    parser.add_argument(
        '-d', '--delay', help='Delay before the i2c should be updated', default=0.0)
    args = parser.parse_args()
    delayInterval = float(args.delay)

    h = hal.component(args.name)
    tempPin = h.newpin("temperature", hal.HAL_FLOAT, hal.HAL_OUT)
    pressPin = h.newpin("pressure",   hal.HAL_FLOAT, hal.HAL_OUT)

    anglexPin = h.newpin("angle-x", hal.HAL_FLOAT, hal.HAL_OUT)
    angleyPin = h.newpin("angle-y", hal.HAL_FLOAT, hal.HAL_OUT)
    anglezPin = h.newpin("angle-z", hal.HAL_FLOAT, hal.HAL_OUT)

    magxPin = h.newpin("mag-x", hal.HAL_FLOAT, hal.HAL_OUT)
    magyPin = h.newpin("mag-y", hal.HAL_FLOAT, hal.HAL_OUT)
    magzPin = h.newpin("mag-z", hal.HAL_FLOAT, hal.HAL_OUT)

    accelxPin = h.newpin("accel-x", hal.HAL_FLOAT, hal.HAL_OUT)
    accelyPin = h.newpin("accel-y", hal.HAL_FLOAT, hal.HAL_OUT)
    accelzPin = h.newpin("accel-z", hal.HAL_FLOAT, hal.HAL_OUT)

    gyroxPin = h.newpin("gyro-x", hal.HAL_FLOAT, hal.HAL_OUT)
    gyroyPin = h.newpin("gyro-y", hal.HAL_FLOAT, hal.HAL_OUT)
    gyrozPin = h.newpin("gyro-z", hal.HAL_FLOAT, hal.HAL_OUT)

    h.ready()

    i2c_bus = int(args.bus_id)  # /dev/i2c-2
    mpu6050 = Adafruit_I2C(0x68, i2c_bus)  # ADC 16bit
    hmc5883 = Adafruit_I2C(0x1e, i2c_bus)  # ADC 12bit
    ms5611 = Adafruit_I2C(0x77, i2c_bus)  # ADC 24bit

    # mpu6050 i2c master enable bit
    mpu6050.write8(0x6A, 0)
    # mpu6050 i2c bypass enable bit
    mpu6050.write8(0x37, 2)
    # mpu6050 turn off sleep mode
    mpu6050.write8(0x6B, 0)

    # hmc5883 continuous mode
    hmc5883.write8(0x02, 0x00)

    # ms5611 reset
    ms5611.writeList(0x1e, [])
    time.sleep(2/1000.0)

    # ms5611 prom
    t = ms5611.readList(0xa2, 2)
    c1 = t[0]*256+t[1]
    t = ms5611.readList(0xa4, 2)
    c2 = t[0]*256+t[1]
    t = ms5611.readList(0xa6, 2)
    c3 = t[0]*256+t[1]
    t = ms5611.readList(0xa8, 2)
    c4 = t[0]*256+t[1]
    t = ms5611.readList(0xaa, 2)
    c5 = t[0]*256+t[1]
    t = ms5611.readList(0xac, 2)
    c6 = t[0]*256+t[1]
    # c1=40127
    # c2=36924
    # c3=23317
    # c4=23282
    # c5=33464
    # c6=28312

    try:
        time.sleep(delayInterval)
        while True:
            # read mpu6050 accelerations in m/s^2
            ax = (mpu6050.readS8(0x3b)*256+mpu6050.readU8(0x3c))/16384.0
            ay = (mpu6050.readS8(0x3d)*256+mpu6050.readU8(0x3e))/16384.0
            az = (mpu6050.readS8(0x3f)*256+mpu6050.readU8(0x40))/16384.0
            # read mpu6050 temperature in C
            tt = (mpu6050.readS8(0x41)*256+mpu6050.readU8(0x42))/340.0 + 36.53
            # read mpu6050 gyroscope in degree/s
            gx = (mpu6050.readS8(0x43)*256+mpu6050.readU8(0x44))/131.0
            gy = (mpu6050.readS8(0x45)*256+mpu6050.readU8(0x46))/131.0
            gz = (mpu6050.readS8(0x47)*256+mpu6050.readU8(0x48))/131.0

            # wait hmc5883 to be ready
            while not hmc5883.readU8(0x09) & 0x01 == 1:
                pass
            # read hmc5883 magnetometer in gauss
            magx = (hmc5883.readS8(0x3)*256+hmc5883.readU8(0x4))/1090.0
            magz = (hmc5883.readS8(0x5)*256+hmc5883.readU8(0x6))/1090.0
            magy = (hmc5883.readS8(0x7)*256+hmc5883.readU8(0x8))/1090.0

            z = math.degrees(math.atan2(magy, magx))
            y = math.degrees(-math.atan2(ax, az))
            x = math.degrees(math.atan2(ay, az))

            ms5611.writeList(0x48, [])
            time.sleep(9/1000.0)
            adc = ms5611.readList(0x00, 3)
            d1 = adc[0]*65536+adc[1]*256+adc[2]
            # d1=9085466

            ms5611.writeList(0x58, [])
            time.sleep(9/1000.0)
            adc = ms5611.readList(0x00, 3)
            d2 = adc[0]*65536+adc[1]*256+adc[2]
            # d2=8569150

            dt = d2 - c5*(1 << 8)
            temp = 2000 + dt*c6/(1 << 23)
            off = c2*(1 << 16) + c4*dt/(1 << 7)
            sens = c1*(1 << 15) + c3*dt/(1 << 8)
            p = (d1*sens/(1 << 21) - off)/(1 << 15)

            # print c1,c2,c3,c4,c5,c6,d1,d2,dt,temp,off,sens,p

            tempPin.value = temp/100.0
            pressPin.value = p/100.0

            anglexPin.value = x
            angleyPin.value = y
            anglezPin.value = z

            magxPin.value = magx
            magyPin.value = magy
            magzPin.value = magz

            accelxPin.value = ax
            accelyPin.value = ay
            accelzPin.value = az

            gyroxPin.value = gx
            gyroyPin.value = gy
            gyrozPin.value = gz

    except:
        print(("exiting HAL component " + args.name))
        h.exit()
